package ucd.csi.comp30290.fall2011.ebbt;

/**
 * This implementation, or rather the <code>calculate</code> method signature we are 
 * forced to implement, is patently bonkers. However, the 'Great Circle' math involved 
 * is correct. 
 * <br>
 * Almost all invocations will get the same line and branch coverage. Only very special 
 * cases will go down the exceptional branches.
 * 
 * @author David Harrison
 */
public class NavigationBlackBox implements BlackBox {
    
    private static final double PI = Math.PI;
    private static final double HALFPI = PI / 2;
    private static final double TWOPI = PI * 2;

    /**
     * For two positions on the earth's surface, calculate the distance between them, 
     * the true course from one to the other (and visa versa) and the mid point.
     * 
     * @param lat1  latitude (degrees) of the first position
     * @param lon1  longitude (degrees) of the first position
     * @param lat2  latitude (degrees) of the second position
     * @param lon2  longitude (degrees) of the second position
     * @param course1 'true' course (degrees) from first position
     * @param course2 'true' course (degrees) from second position
     * 
     * @return and array containing (in order) 
     *      [0] the distance (nautical miles) between the two positions
     *      [1] the 'true' course (degrees) from the first to the second position
     *      [2] the 'true' course (degrees) from the second to the first position
     *      [3] latitude (degrees) of the midpoint between the two positions
     *      [4] longitude (degrees) of the midpoint between the two positions
     *      [5] latitude (degrees) of the intersection of the two courses 
     *      [6] longitude (degrees) of the intersection of the two courses 
     *      
     * @throws <code>java.lang.RuntimException</code> if the calculated intersection
     *      is either infinite or ambiguous
     */
    public double[] calculate(double lat1, double lon1, double lat2, double lon2, double course1, double course2) {
        double[] midpoint = midpoint(lat1, lon2, lat2, lon2);
        double[] intersection = intersection(lat1, lon2, lat2, lon2, course1, course2);
        return new double[] {
            distance(lat1, lon1, lat2, lon2),
            trueCourse(lat1, lon1, lat2, lon2),
            trueCourse(lat2, lon2, lat1, lon1),
            midpoint[0], midpoint[1],
            intersection[0], intersection[1] };
    }
    
    private static double radiansToNauticalMiles(double radians) {
        return Math.toDegrees(radians) * 60;
    }
    
    // Euclidean modulus
    public static double mod(double x, double y) {
        double mod = x - y * Math.floor(x / y);
        return mod < 0 ? mod + y : mod;
    }

    private double trueCourse(double lat1, double lon1, double lat2, double lon2) {
        double dLng = Math.toRadians(lon2) - Math.toRadians(lon1);
        return Math.atan2(
            Math.sin(dLng),
            Math.cos(Math.toRadians(lat1)) * Math.tan(Math.toRadians(lat2))
                - Math.sin(Math.toRadians(lat1)) * Math.cos(dLng));
    }

    private double distance(double lat1, double lon1, double lat2, double lon2) {
        double phi1 = HALFPI - Math.toRadians(lat1);
        double phi2 = HALFPI - Math.toRadians(lat2);
        double dTheta = Math.toRadians(lon2) - Math.toRadians(lon1);
        return radiansToNauticalMiles(
            Math.acos(Math.cos(phi1) * Math.cos(phi2) + Math.sin(phi1)
                * Math.sin(phi2) * Math.cos(dTheta)));
    }

    private double[] midpoint(double lat1, double lon1, double lat2, double lon2) {
        lat1 = Math.toRadians(lat1);
        lon1 = Math.toRadians(lon1);
        lat2 = Math.toRadians(lat1);
        lon2 = Math.toRadians(lon2);
        double dLon = (lon2 - lon1);
        double Bx = Math.cos(lat2) * Math.cos(dLon);
        double By = Math.cos(lat2) * Math.sin(dLon);
        double lat3 = Math.atan2(Math.sin(lat1)+Math.sin(lat2),
            Math.sqrt( (Math.cos(lat1)+Bx)*(Math.cos(lat1)+Bx) + By*By) );
        double lon3 = lon1 + Math.atan2(By, Math.cos(lat1) + Bx);
        lon3 = (lon3+3*PI) % (TWOPI) - PI; 
        return new double[] { Math.toDegrees(lat3), Math.toDegrees(lon3) }; 
    }
    
    public double[] intersection(
            double lat1, double lon1, double lat2, double lon2, double course1, double course2) {
        
        double latrad1 = Math.toRadians(lat1);
        double lonrad1 = Math.toRadians(lon1);

        double latrad2 = Math.toRadians(lat1);
        double lonrad2 = Math.toRadians(lon2);

        double crs13 = Math.toRadians(course1);
        double crs23 = Math.toRadians(course2);

        double w = lonrad2 - lonrad1;
        double v = latrad1 - latrad2;
        double s = 2 * Math.asin(Math.sqrt((Math.sin(v / 2) * Math.sin(v / 2))
            + (Math.cos(latrad1) * Math.cos(latrad2) * Math.sin(w / 2) * Math.sin(w / 2)))); 

        double crs12;
        double crs21;

        if (Math.sin(lonrad1 - lonrad2) < 0) {
            crs12 = Math.acos((Math.sin(latrad2) - Math.sin(latrad1) * Math.cos(s))
                / (Math.sin(s) * Math.cos(latrad1)));
        } else {
            crs12 = TWOPI - Math.acos((Math.sin(latrad2) - Math.sin(latrad1) * Math.cos(s))
                / (Math.sin(s) * Math.cos(latrad1)));
        }

        if (Math.sin(lonrad2 - lonrad1) < 0) {
            crs21 = Math.acos((Math.sin(latrad1) - Math.sin(latrad2)
               * Math.cos(s)) / (Math.sin(s) * Math.cos(latrad2)));
        } else {
            crs21 = TWOPI - Math.acos((Math.sin(latrad1) - Math.sin(latrad2)
               * Math.cos(s)) / (Math.sin(s) * Math.cos(latrad2)));
        }

        double ang1 = mod(crs13 - crs12 + PI, TWOPI) - PI;
        double ang2 = mod(crs21 - crs23 + PI, TWOPI) - PI;

        double tol = 0.000000000000001;

        // Infinite Intersections
        if (Math.sin(ang1) == 0 && Math.sin(ang2) == 0) 
            throw new RuntimeException("Infinite Intersections!");
        
        // Ambiguous Intersection
        if (Math.sin(ang1) * Math.sin(ang2) <= Math.sqrt(tol)) 
            throw new RuntimeException("Ambiguous intersection!");
            
        ang1 = Math.abs(ang1);
        ang2 = Math.abs(ang2);
        double ang3 = Math.acos(Math.sin(ang1) * Math.sin(ang2)
            * Math.cos(s) - Math.cos(ang1) * Math.cos(ang2));
        double dst13 = Math.asin(Math.sin(ang2) * Math.sin(s) / Math.sin(ang3));
        double latrad3 = Math.asin(Math.sin(latrad1) * Math.cos(dst13)
            + Math.cos(latrad1) * Math.sin(dst13) * Math.cos(crs13));
        double lonrad3 = lonrad1 + Math.asin(Math.sin(crs13) * Math.sin(dst13) / Math.cos(latrad3));
        lonrad3 = mod(lonrad3 + PI, TWOPI) - PI;
        
        return new double[] { Math.toDegrees(latrad3), Math.toDegrees(lonrad3) };
    }
}
